#include <yaml-cpp/yaml.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <limits>
#include <memory>
#include <queue>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>


#include <thread>
#include <atomic>
#include <mutex>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"

#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "std_msgs/msg/bool.hpp"  // 添加 Bool 消息头文件
#include "std_srvs/srv/trigger.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

// PCL 相关头文件
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "interface/srv/hearts.hpp"
#include "interface/srv/nav_msg.hpp"
#include "interface/srv/relocalize.hpp"
#include "interface/srv/stop_nav.hpp"

using namespace std::chrono_literals;
// void pLine() {
//     RCLCPP_INFO(this->get_logger(), "------------------------------");
// }

rclcpp::Time last_goal_publish_time_;

// 单个拓扑点类型
struct road_node {
    std::string node_name;
    int floor_level;                           // 楼层
    std::vector<float> param;                  // x,y,z,yaw
    std::vector<std::string> connected_nodes;  // 关联节点
    std::string id;                            // 属性
    std::vector<double> length_list;           // 代价长度
};

// 单个地图拓扑边类型
struct map_edge {
    std::string map_name;                      // 地图名称
    int floor_level;                           // 楼层
    std::vector<std::string> connected_nodes;  // 关联楼层
    std::vector<double> length_list;           // 代价长度
};

struct heart_msg{
    std::string back_time;
    std::string call_start_floor;
    std::string call_stop_floor;
    std::string curr_floor;
    double nav_dis;
    bool nav_finish;
};
heart_msg back_heart_msg;

void init_heart_msg(){
    back_heart_msg.back_time = "";
    back_heart_msg.call_start_floor = "";
    back_heart_msg.call_stop_floor = "";
    back_heart_msg.curr_floor = "";
    back_heart_msg.nav_dis = 9999.0;
    back_heart_msg.nav_finish = false;
}


std::map<std::string, road_node> road_network;  // 路网数据结构
std::map<std::string, map_edge> map_topology;   // 地图拓扑数据结构

// A*搜索节点结构
struct AStarNode {
    std::string node_name;
    double g_cost;  // 从起点到当前节点的实际代价
    double h_cost;  // 从当前节点到终点的启发式代价(欧氏距离)
    double f_cost;  // f = g + h
    std::string parent;

    AStarNode(const std::string& name, double g, double h, const std::string& p) : node_name(name), g_cost(g), h_cost(h), f_cost(g + h), parent(p) {}

    bool operator>(const AStarNode& other) const { return f_cost > other.f_cost; }
};

class TopoNavNode : public rclcpp::Node {
   public:
    TopoNavNode() : Node("topo_nav_node"), navigation_active_(false), stop_navigation_(false) {
        this->declare_parameter<std::string>("map_frame", "map");         // 声明地图坐标系参数
        this->declare_parameter<std::string>("base_frame", "base_link");  // 声明机器人坐标系参数
        this->declare_parameter<std::string>("config_dir", "");           // 声明配置文件目录
        this->declare_parameter<std::string>("pcd_dir", "");              // 声明PCD文件目录
        this->declare_parameter<std::string>("map_topo_name", "");        // 声明地图拓扑名称
        map_frame_ = this->get_parameter("map_frame").as_string();
        base_frame_ = this->get_parameter("base_frame").as_string();
        config_dir_ = this->get_parameter("config_dir").as_string();
        pcd_dir_ = this->get_parameter("pcd_dir").as_string();
        map_topo_name_ = this->get_parameter("map_topo_name").as_string();

        pLine();
        std::cout << "map_frame: " << map_frame_ << std::endl;
        std::cout << "base_frame: " << base_frame_ << std::endl;
        std::cout << "config_dir: " << config_dir_ << std::endl;
        std::cout << "pcd_dir: " << pcd_dir_ << std::endl;
        std::cout << "map_topo_name: " << map_topo_name_ << std::endl;
        pLine();

        // 初始化TF
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        // TODO 初始化数据

        // 开始导航
        nav_service_ = this->create_service<interface::srv::NavMsg>(
            "nav_server", std::bind(&TopoNavNode::nav_service_callback, this, std::placeholders::_1, std::placeholders::_2));  // 创建服务服务器

        // 停止导航
        stop_nav_service_ = this->create_service<interface::srv::StopNav>(
            "stop_nav_server",
            std::bind(&TopoNavNode::stop_nav_service_callback, this, std::placeholders::_1, std::placeholders::_2));  // 创建停止导航服务服务器

        // 心跳服务
        hearts_service_ = this->create_service<interface::srv::Hearts>(
            "hearts_server",
            std::bind(&TopoNavNode::hearts_service_callback, this, std::placeholders::_1, std::placeholders::_2));  // 创建心跳服务服务器

        // 初始化lio
        re_fast_lio_service_ = this->create_client<std_srvs::srv::Trigger>("/reinitialize");
        // 初始化localizer
        reload_map_service_ = this->create_client<interface::srv::Relocalize>("/localizer/relocalize");

        goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/goal_pose", 10);               // 创建发布器,用于发布导航目标
        path_publisher_ = this->create_publisher<nav_msgs::msg::Path>("/topo_nav_path", 10);                       // 创建发布器,用于发布完整导航路径
        topo_cloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/topo_network_cloud", 10);  // 拓扑路网点云发布器
        scan_filter_publisher_ = this->create_publisher<std_msgs::msg::Bool>("/filter_center", 10);          // 扫描滤波触发发布器

        // 移除 Nav2 action 客户端初始化
        // nav2_action_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this, "navigate_to_pose");

        m_timer_pose = this->create_wall_timer(100ms, std::bind(&TopoNavNode::timerCB_robot_pose, this));  // 10hz

        // 获取配置文件路径参数

        last_goal_publish_time_ = this->now();
        
        // 新增：初始化位置检测变量
        last_robot_position_ = geometry_msgs::msg::Point();
        last_position_check_time_ = this->now();
    }

    ~TopoNavNode() {
        // 确保导航线程结束
        stop_navigation_ = true;
        if (navigation_thread_.joinable()) {
            navigation_thread_.join();
        }
    }

   private:
    rclcpp::Service<interface::srv::NavMsg>::SharedPtr nav_service_;        // 导航服务
    rclcpp::Service<interface::srv::StopNav>::SharedPtr stop_nav_service_;  // 停止导航服务
    rclcpp::Service<interface::srv::Hearts>::SharedPtr hearts_service_;     // 心跳服务

    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr re_fast_lio_service_;     // 初始化lio服务
    rclcpp::Client<interface::srv::Relocalize>::SharedPtr reload_map_service_;  // 初始化地图服务

    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;      // 导航目标发布器
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;                  // 导航路径发布器（用于可视化）
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr topo_cloud_publisher_;  // 拓扑路网点云发布器
    rclcpp::TimerBase::SharedPtr m_timer_pose;

    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr scan_filter_publisher_;  // 扫描滤波触发发布器
    
    // 移除 Nav2 action 客户端声明
    // rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr nav2_action_client_;
    
    // TF相关
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    // 机器人当前位置
    geometry_msgs::msg::PoseStamped robot_pose_;

    // 坐标系名称
    std::string map_frame_;
    std::string base_frame_;

    // 配置文件和PCD文件目录
    std::string map_topo_name_;
    std::string config_dir_;
    std::string pcd_dir_;

    // 导航状态管理
    std::atomic<bool> navigation_active_;
    std::atomic<bool> stop_navigation_;
    std::thread navigation_thread_;
    std::mutex navigation_mutex_;

    // 新增：位置变化检测相关
    geometry_msgs::msg::Point last_robot_position_;  // 上次记录的机器人位置
    rclcpp::Time last_position_check_time_;          // 上次检查位置的时间
    std::mutex position_check_mutex_;                 // 保护位置检测变量的互斥锁



   private:
    void nav_service_callback(const std::shared_ptr<interface::srv::NavMsg::Request> request,
                              std::shared_ptr<interface::srv::NavMsg::Response> response);

    void stop_nav_service_callback(const std::shared_ptr<interface::srv::StopNav::Request> request,
                                   std::shared_ptr<interface::srv::StopNav::Response> response);

    void hearts_service_callback(const std::shared_ptr<interface::srv::Hearts::Request> request,
                                 std::shared_ptr<interface::srv::Hearts::Response> response);

    bool load_map_topology(const std::string& config_file);   // 加载地图拓扑
    bool load_floor_topology(const std::string& floor_name);  // 根据楼层名称加载对应的路网

    void publish_topology_as_cloud();  // 发布拓扑点云

    bool call_relio_service();                                                                          // 调用重定位服务
    bool call_change_pcd_service(const std::string& pcd_name, const std::vector<float>& initial_pose);  // 调用更换点云服务

    // 新增：拓扑插值相关函数
    void interpolate_topology_edges(double max_interpolation_distance = 1.0);                         // 

    // 辅助函数
    std::string find_nearest_node(float x, float y, float z);                                                  // 查找最近的拓扑节点
    double calculate_euclidean_distance(float x1, float y1, float z1, float x2, float y2, float z2);           // 计算欧氏距离
    std::vector<std::string> astar_search(const std::string& start, const std::string& goal);                  // A*搜索算法
    void publish_navigation_path(const std::vector<std::string>& path, const std::vector<float>& final_pose);  // 发布导航路径

    void timerCB_robot_pose();  // 定时获取机器人位置

    // 辅助函数 - 角度和距离计算
    double quaternion_to_yaw(double qx, double qy, double qz, double qw);            // 四元数转yaw角
    double normalize_angle_difference(double angle1, double angle2);                 // 归一化角度差
    double calculate_distance_to_pose(const geometry_msgs::msg::PoseStamped& pose);  // 计算到目标位姿的距离
    bool check_waypoint_reached(const geometry_msgs::msg::PoseStamped& goal_pose,
                                double distance_threshold,
                                double angle_threshold);                                     // 检查是否到达航点
    geometry_msgs::msg::PoseStamped create_goal_pose(float x, float y, float z, float yaw);  // 创建目标位姿

    // 跨楼层导航辅助函数
    std::vector<std::string> search_floor_path(const std::string& start_map, const std::string& goal_map);  // 搜索楼层间路径
    std::string find_node_by_id(const std::string& id);                                                     // 根据ID查找节点名称
    bool navigate_to_node(const std::string& target_node_name, const std::string& direction,const bool is_stairs);               // 导航到指定节点
    bool execute_cross_floor_navigation(const std::vector<std::string>& floor_path,
                                        const std::string& start_map,
                                        const std::string& goal_map,
                                        const std::vector<float>& final_pose);  // 执行跨楼层导航

    // 新增：后台导航线程函数
    void navigation_thread_func(const std::shared_ptr<interface::srv::NavMsg::Request> request);
    
    // 新增：检查是否需要停止导航
    bool should_stop_navigation() const { return stop_navigation_.load(); }

    // 新增：检查机器人是否移动
    bool has_robot_moved(double threshold = 0.05);  // 默认5cm阈值
    
    // 新增：更新位置检测基准
    void update_position_baseline();

    // 新增：取消Nav2导航
    void stop_current_navigation();
    void pLine();
};
/****************************************************************************************************** */
void TopoNavNode::pLine() {
    RCLCPP_INFO(this->get_logger(), "------------------------------");
}



// 拓扑边插值函数
void TopoNavNode::interpolate_topology_edges(double max_interpolation_distance) {
    RCLCPP_INFO(this->get_logger(), "Starting topology edge interpolation with max distance: %.2f m", max_interpolation_distance);

    // 复制原始路网，用于创建新节点
    std::map<std::string, road_node> original_network = road_network;

    // 用于记录已处理的边，避免重复
    std::set<std::pair<std::string, std::string>> processed_edges;

    int total_interpolated_nodes = 0;

    // 遍历所有原始节点
    for (const auto& [node_name, node_data] : original_network) {
        // 遍历该节点的所有邻居
        for (size_t i = 0; i < node_data.connected_nodes.size(); ++i) {
            const std::string& neighbor_name = node_data.connected_nodes[i];

            // 检查邻居是否存在
            if (original_network.find(neighbor_name) == original_network.end()) {
                continue;
            }

            // 创建边的唯一标识（按字典序排序，避免重复）
            std::pair<std::string, std::string> edge_key;
            if (node_name < neighbor_name) {
                edge_key = {node_name, neighbor_name};
            } else {
                edge_key = {neighbor_name, node_name};
            }

            // 如果已处理过此边，跳过
            if (processed_edges.find(edge_key) != processed_edges.end()) {
                continue;
            }
            processed_edges.insert(edge_key);

            // 获取两个节点的位置
            const auto& neighbor_data = original_network[neighbor_name];
            float start_x = node_data.param[0];
            float start_y = node_data.param[1];
            float end_x = neighbor_data.param[0];
            float end_y = neighbor_data.param[1];

            // 计算距离
            double distance = calculate_euclidean_distance(start_x, start_y, 0.0f, end_x, end_y, 0.0f);

            // 计算需要插入的节点数量
            int num_interpolated = static_cast<int>(distance / max_interpolation_distance);

            if (num_interpolated > 0) {
                // RCLCPP_INFO(this->get_logger(), "Interpolating edge %s - %s (distance: %.2f m) with %d nodes", node_name.c_str(),
                //             neighbor_name.c_str(), distance, num_interpolated);

                std::vector<std::string> created_nodes;

                // 创建插值节点
                for (int j = 1; j <= num_interpolated; ++j) {
                    double ratio = static_cast<double>(j) / (num_interpolated + 1);
                    float interp_x = start_x + ratio * (end_x - start_x);
                    float interp_y = start_y + ratio * (end_y - start_y);

                    // 生成插值节点名称
                    std::string interp_name = "int_" + node_name + "_" + neighbor_name + "_" + std::to_string(j);
                    created_nodes.push_back(interp_name);

                    // 创建插值节点
                    road_node interp_node;
                    interp_node.node_name = interp_name;
                    interp_node.floor_level = node_data.floor_level;
                    interp_node.param = {interp_x, interp_y, 0.0f, 0.0f};
                    interp_node.id = "1";  // 插值节点ID设为1

                    // 设置邻居关系
                    if (j > 1) {
                        // 连接到前一个插值节点
                        interp_node.connected_nodes.push_back(created_nodes[j - 2]);
                    } else {
                        // 第一个插值节点连接到起始节点
                        interp_node.connected_nodes.push_back(node_name);
                    }

                    if (j < num_interpolated) {
                        // 连接到后一个插值节点（先占位）
                        std::string next_interp = "int_" + node_name + "_" + neighbor_name + "_" + std::to_string(j + 1);
                        interp_node.connected_nodes.push_back(next_interp);
                    } else {
                        // 最后一个插值节点连接到结束节点
                        interp_node.connected_nodes.push_back(neighbor_name);
                    }

                    // 添加到路网
                    road_network[interp_name] = interp_node;
                    total_interpolated_nodes++;

                    RCLCPP_DEBUG(this->get_logger(), "  Created interpolation node: %s at (%.2f, %.2f)", interp_name.c_str(), interp_x, interp_y);
                }

                // 更新起始节点的邻居（移除直接连接，添加第一个插值节点）
                auto& start_node = road_network[node_name];
                auto& start_neighbors = start_node.connected_nodes;
                auto it = std::find(start_neighbors.begin(), start_neighbors.end(), neighbor_name);
                if (it != start_neighbors.end()) {
                    *it = created_nodes.front();
                }

                // 更新结束节点的邻居（移除直接连接，添加最后一个插值节点）
                auto& end_node = road_network[neighbor_name];
                auto& end_neighbors = end_node.connected_nodes;
                it = std::find(end_neighbors.begin(), end_neighbors.end(), node_name);
                if (it != end_neighbors.end()) {
                    *it = created_nodes.back();
                }
            }
        }
    }

    // 重新计算所有节点的length_list
    for (auto& [node_name, node_data] : road_network) {
        node_data.length_list.clear();

        for (const auto& connected_node_name : node_data.connected_nodes) {
            auto it = road_network.find(connected_node_name);
            if (it != road_network.end()) {
                const auto& connected_node = it->second;
                double dist = calculate_euclidean_distance(node_data.param[0], node_data.param[1], node_data.param[2], connected_node.param[0],
                                                           connected_node.param[1], connected_node.param[2]);
                node_data.length_list.push_back(dist);
            } else {
                node_data.length_list.push_back(1.0);
            }
        }
    }

    RCLCPP_INFO(this->get_logger(), "✓ Interpolation complete: created %d new nodes, total nodes: %zu", total_interpolated_nodes,
                road_network.size());
}

bool TopoNavNode::load_floor_topology(const std::string& config_file) {
    try {
        // 检查文件是否存在
        std::ifstream file(config_file);
        if (!file.is_open()) {
            RCLCPP_ERROR(this->get_logger(), "Cannot open config file: %s", config_file.c_str());
            return false;
        }
        file.close();

        // 加载YAML文件
        YAML::Node config = YAML::LoadFile(config_file);

        // 清空现有的路网数据
        road_network.clear();

        // 检查是否有node_list
        if (!config["node_list"]) {
            RCLCPP_ERROR(this->get_logger(), "No 'node_list' found in config file");
            return false;
        }

        // 遍历所有节点
        for (const auto& node : config["node_list"]) {
            if (!node["node_name"] || !node["floor"] || !node["param"]) {
                RCLCPP_WARN(this->get_logger(), "Skipping invalid node entry - missing node_name, floor, or param");
                continue;
            }

            std::string node_name = node["node_name"].as<std::string>();
            int floor_level = node["floor"].as<int>();

            // 创建road_node结构
            road_node road_node_data;
            road_node_data.node_name = node_name;
            road_node_data.floor_level = floor_level;

            // 获取param数组
            const auto& params = node["param"];
            if (params.size() < 6) {
                RCLCPP_WARN(this->get_logger(), "Node '%s' has insufficient parameters, expected at least 6", node_name.c_str());
                continue;
            }

            // 解析位置和方向参数 [x, y, z, yaw]
            road_node_data.param.clear();
            road_node_data.param.push_back(params[0].as<float>());  // x
            road_node_data.param.push_back(params[1].as<float>());  // y
            road_node_data.param.push_back(params[2].as<float>());  // z
            road_node_data.param.push_back(params[3].as<float>());  // yaw

            // 解析连接的节点列表
            road_node_data.connected_nodes.clear();
            std::string connected_nodes_str = params[4].as<std::string>();

            // 分割连接节点字符串（以逗号分隔）
            std::stringstream ss(connected_nodes_str);
            std::string token;
            while (std::getline(ss, token, ',')) {
                // 去除前后空格
                token.erase(0, token.find_first_not_of(" \t"));
                token.erase(token.find_last_not_of(" \t") + 1);
                if (!token.empty()) {
                    road_node_data.connected_nodes.push_back(token);
                }
            }

            // 解析节点ID
            road_node_data.id = params[5].as<std::string>();

            // 暂时初始化空的长度列表，稍后计算欧氏距离
            road_node_data.length_list.clear();

            // 存储到road_network中
            road_network[node_name] = road_node_data;

            RCLCPP_DEBUG(this->get_logger(), "Loaded road node: %s (floor: %d) at (%.2f, %.2f, %.2f) with %zu connections", node_name.c_str(),
                         floor_level, road_node_data.param[0], road_node_data.param[1], road_node_data.param[2],
                         road_node_data.connected_nodes.size());
        }

        RCLCPP_INFO(this->get_logger(), "Loaded %zu road network nodes", road_network.size());

        // 计算每个节点到其连接节点的欧氏距离
        for (auto& [node_name, road_node_data] : road_network) {
            road_node_data.length_list.clear();

            for (const auto& connected_node_name : road_node_data.connected_nodes) {
                // 查找连接的节点
                auto it = road_network.find(connected_node_name);
                if (it != road_network.end()) {
                    const auto& connected_node = it->second;

                    // 计算欧氏距离 (x, y, z)
                    float dx = road_node_data.param[0] - connected_node.param[0];
                    float dy = road_node_data.param[1] - connected_node.param[1];
                    float dz = road_node_data.param[2] - connected_node.param[2];
                    double distance = std::sqrt(dx * dx + dy * dy + dz * dz);

                    road_node_data.length_list.push_back(distance);

                    RCLCPP_DEBUG(this->get_logger(), "Distance from '%s' to '%s': %.3f m", node_name.c_str(), connected_node_name.c_str(), distance);
                } else {
                    RCLCPP_WARN(this->get_logger(), "Connected node '%s' not found for node '%s', using default distance 1.0",
                                connected_node_name.c_str(), node_name.c_str());
                    road_node_data.length_list.push_back(1.0);
                }
            }
        }

        // **新增：检测是否为楼层地图并进行插值**
        // 判断依据：config_file名称中包含"_topo"且不包含"stairs"等关键词
        bool is_floor_map = false;
        if (config_file.find("_topo.yaml") != std::string::npos) {
            // 检查是否包含楼梯关键词
            if (config_file.find("stairs") == std::string::npos && config_file.find("stair") == std::string::npos) {
                is_floor_map = true;
            }
        }

        if (is_floor_map) {
            RCLCPP_INFO(this->get_logger(), "Detected floor map, performing topology interpolation...");
            interpolate_topology_edges(2.0);  // 使用5米作为最大插值距离
        } else {
            RCLCPP_INFO(this->get_logger(), "Non-floor map detected, skipping interpolation");
        }

        // 打印加载的路网结构（用于调试）
        for (const auto& [node_name, road_node_data] : road_network) {
            std::string connections = "";
            for (size_t i = 0; i < road_node_data.connected_nodes.size(); ++i) {
                connections += road_node_data.connected_nodes[i];
                connections += "(";
                connections += std::to_string(road_node_data.length_list[i]);
                connections += "m)";
                if (i < road_node_data.connected_nodes.size() - 1) {
                    connections += ", ";
                }
            }
            // RCLCPP_INFO(this->get_logger(), "Node '%s' (ID: %s, Floor: %d) at pos[%.2f, %.2f, %.2f, %.2f] connects to: [%s]", node_name.c_str(),
            //             road_node_data.id.c_str(), road_node_data.floor_level, road_node_data.param[0], road_node_data.param[1],
            //             road_node_data.param[2], road_node_data.param[3], connections.c_str());
        }

        // 新增：发布拓扑路网点云
        publish_topology_as_cloud();

        return true;

    } catch (const YAML::Exception& e) {
        RCLCPP_ERROR(this->get_logger(), "YAML parsing error: %s", e.what());
        return false;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Error loading road network: %s", e.what());
        return false;
    }
}

bool TopoNavNode::load_map_topology(const std::string& config_file) {
    try {
        // 检查文件是否存在
        std::ifstream file(config_file);
        if (!file.is_open()) {
            RCLCPP_ERROR(this->get_logger(), "Cannot open config file: %s", config_file.c_str());
            return false;
        }
        file.close();

        // 加载YAML文件
        YAML::Node config = YAML::LoadFile(config_file);

        // 清空现有的地图拓扑数据
        map_topology.clear();

        // 检查是否有node_list
        if (!config["node_list"]) {
            RCLCPP_ERROR(this->get_logger(), "No 'node_list' found in config file");
            return false;
        }

        // 遍历所有节点
        for (const auto& node : config["node_list"]) {
            if (!node["node_name"] || !node["floor"] || !node["param"]) {
                RCLCPP_WARN(this->get_logger(), "Skipping invalid node entry - missing node_name, floor, or param");
                continue;
            }

            std::string node_name = node["node_name"].as<std::string>();
            int floor_level = node["floor"].as<int>();

            // 创建map_edge结构
            map_edge edge;
            edge.map_name = node_name;
            edge.floor_level = floor_level;

            // 获取连接的节点列表
            for (const auto& param : node["param"]) {
                std::string connected_node = param.as<std::string>();
                edge.connected_nodes.push_back(connected_node);

                // 暂时设置默认长度为1.0，后续可以根据实际情况调整
                edge.length_list.push_back(1.0);
            }

            // 存储到map_topology中
            map_topology[node_name] = edge;

            RCLCPP_DEBUG(this->get_logger(), "Loaded map node: %s (floor: %d) with %zu connections", node_name.c_str(), floor_level,
                         edge.connected_nodes.size());
        }

        RCLCPP_INFO(this->get_logger(), "Loaded %zu map topology nodes", map_topology.size());

        // 打印加载的拓扑结构（用于调试）
        for (const auto& [map_name, edge] : map_topology) {
            std::string connections = "";
            for (size_t i = 0; i < edge.connected_nodes.size(); ++i) {
                connections += edge.connected_nodes[i];
                if (i < edge.connected_nodes.size() - 1) {
                    connections += ", ";
                }
            }

            std::string floor_type = (edge.floor_level == 0) ? "stairs" : "floor " + std::to_string(edge.floor_level);

            RCLCPP_INFO(this->get_logger(), "Map '%s' (%s) connects to: [%s]", map_name.c_str(), floor_type.c_str(), connections.c_str());
        }

        return true;

    } catch (const YAML::Exception& e) {
        RCLCPP_ERROR(this->get_logger(), "YAML parsing error: %s", e.what());
        return false;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Error loading map topology: %s", e.what());
        return false;
    }
}

// 获取机器人位置
void TopoNavNode::timerCB_robot_pose() {
    try {
        // 查找从map到base_link的变换
        geometry_msgs::msg::TransformStamped transform_stamped;
        transform_stamped = tf_buffer_->lookupTransform(map_frame_, base_frame_,
                                                        tf2::TimePointZero);  // 获取最新的变换

        // 更新机器人位置
        robot_pose_.header.stamp = this->now();
        robot_pose_.header.frame_id = map_frame_;

        // 从变换中提取位置信息
        robot_pose_.pose.position.x = transform_stamped.transform.translation.x;
        robot_pose_.pose.position.y = transform_stamped.transform.translation.y;
        robot_pose_.pose.position.z = transform_stamped.transform.translation.z;

        // 从变换中提取姿态信息
        robot_pose_.pose.orientation.x = transform_stamped.transform.rotation.x;
        robot_pose_.pose.orientation.y = transform_stamped.transform.rotation.y;
        robot_pose_.pose.orientation.z = transform_stamped.transform.rotation.z;
        robot_pose_.pose.orientation.w = transform_stamped.transform.rotation.w;

        // std::cout << "Updated Robot Pose: (" << robot_pose_.pose.position.x << ", " << robot_pose_.pose.position.y << ", "
        //           << robot_pose_.pose.position.z << ")" << std::endl;

    } catch (const tf2::TransformException& ex) {
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Could not transform %s to %s: %s", base_frame_.c_str(),
                             map_frame_.c_str(), ex.what());
    }
}

void TopoNavNode::stop_nav_service_callback(const std::shared_ptr<interface::srv::StopNav::Request> request,
                                            std::shared_ptr<interface::srv::StopNav::Response> response) {

    stop_current_navigation();
    init_heart_msg();
    RCLCPP_INFO(this->get_logger(), "Received stop navigation request");
    
    if(request->stop_nav == false) {
        response->success = false;
        response->message = "Stop flag not set in request";
        RCLCPP_WARN(this->get_logger(), "Stop flag not set in request");
        return;
    }
    if (!navigation_active_.load()) {
        response->success = false;
        response->message = "No active navigation to stop";
        RCLCPP_WARN(this->get_logger(), "No active navigation to stop");
        return;
    }
    
    // 设置停止标志
    stop_navigation_ = true;
    
    // 等待导航线程结束
    if (navigation_thread_.joinable()) {
        navigation_thread_.join();
    }
    
    response->success = true;
    response->message = "Navigation stopped successfully";
    RCLCPP_INFO(this->get_logger(), "Navigation stopped successfully");
}

//TODO 心跳回调
void TopoNavNode::hearts_service_callback(const std::shared_ptr<interface::srv::Hearts::Request> request,
                                          std::shared_ptr<interface::srv::Hearts::Response> response) {
    // RCLCPP_INFO(this->get_logger(), "Heartbeat received %s", request->call_time.c_str());
    ///////////////////////////////
    response->back_time = "";
    response->call_start_floor = back_heart_msg.call_start_floor;
    response ->call_stop_floor = back_heart_msg.call_stop_floor;
    response -> curr_floor = back_heart_msg.curr_floor;
    response->nav_dis = std::to_string(back_heart_msg.nav_dis);
    response->nav_finish = back_heart_msg.nav_finish;
    
    // 心跳服务保持活动，目前无需实现
}

void TopoNavNode::publish_topology_as_cloud() {
    if (road_network.empty()) {
        RCLCPP_WARN(this->get_logger(), "Road network is empty, cannot publish topology cloud");
        return;
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr topo_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

    // 添加拓扑节点点云
    for (const auto& [node_name, node_data] : road_network) {
        pcl::PointXYZRGB point;
        point.x = node_data.param[0];
        point.y = node_data.param[1];
        point.z = node_data.param[2];

        // 根据节点ID设置颜色
        if (node_data.id == "1") {
            point.r = 255;
            point.g = 0;
            point.b = 0;  // 红色 (ID1 - 楼梯起点)
        } else if (node_data.id == "2") {
            point.r = 0;
            point.g = 255;
            point.b = 0;  // 绿色 (ID2 - 楼梯终点)
        } else {
            point.r = 255;
            point.g = 255;
            point.b = 0;  // 黄色 (普通节点)
        }

        topo_cloud->points.push_back(point);
    }

    // 将点云转化为ros格式
    sensor_msgs::msg::PointCloud2 topo_cloud_msg;
    pcl::toROSMsg(*topo_cloud, topo_cloud_msg);
    topo_cloud_msg.header.stamp = this->now();
    topo_cloud_msg.header.frame_id = map_frame_;

    // 发布点云
    topo_cloud_publisher_->publish(topo_cloud_msg);
    // RCLCPP_INFO(this->get_logger(), "✓ Published topology network cloud with %zu points", topo_cloud->points.size());
    // RCLCPP_INFO(this->get_logger(), "  - Red points: ID=1 (stair start)");
    // RCLCPP_INFO(this->get_logger(), "  - Green points: ID=2 (stair end)");
    // RCLCPP_INFO(this->get_logger(), "  - Yellow points: Normal nodes");
    // RCLCPP_INFO(this->get_logger(), "  - Blue points: Connection lines");
}

// TODO fastlio初始化
bool TopoNavNode::call_relio_service() {
    // 检查服务是否可用
    if (!re_fast_lio_service_->wait_for_service(std::chrono::seconds(5))) {
        RCLCPP_ERROR(this->get_logger(), "Service /reinitialize not available after waiting");
        return false;
    }

    // 创建请求
    auto request = std::make_shared<std_srvs::srv::Trigger::Request>();

    RCLCPP_INFO(this->get_logger(), "Calling /reinitialize service...");

    // 发送异步请求
    auto result_future = re_fast_lio_service_->async_send_request(request);

    // std::this_thread::sleep_for(2000ms);
    // return true;
    // // 使用 wait_for 等待响应（避免使用 spin_until_future_complete）
    auto status = result_future.wait_for(std::chrono::seconds(5));
    
    if (status == std::future_status::ready) {
        auto response = result_future.get();

        if (response->success) {
            RCLCPP_INFO(this->get_logger(), "Reinitialize service call successful: %s", response->message.c_str());
            return true;
        } else {
            RCLCPP_WARN(this->get_logger(), "Reinitialize service call failed: %s", response->message.c_str());
            return false;
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to call /reinitialize service - timeout or error");
        return false;
    }
}

// TODO localization从新加载地图
bool TopoNavNode::call_change_pcd_service(const std::string& pcd_name, const std::vector<float>& initial_pose) {

    if (!re_fast_lio_service_->wait_for_service(std::chrono::seconds(5))) {
        RCLCPP_ERROR(this->get_logger(), "Service /reinitialize not available after waiting");
        return false;
    }

    auto request1 = std::make_shared<std_srvs::srv::Trigger::Request>();

    RCLCPP_INFO(this->get_logger(), "Calling /reinitialize service...");

    // 发送异步请求
    auto result_future1 = re_fast_lio_service_->async_send_request(request1);

    // return true;
    // // 使用 wait_for 等待响应（避免使用 spin_until_future_complete）
    auto status1 = result_future1.wait_for(std::chrono::seconds(5));
    
    if (status1 == std::future_status::ready) {
        auto response = result_future1.get();

        if (response->success) {
            RCLCPP_INFO(this->get_logger(), "Reinitialize service call successful: %s", response->message.c_str());
            // return true;
        } else {
            RCLCPP_WARN(this->get_logger(), "Reinitialize service call failed: %s", response->message.c_str());
            return false;
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to call /reinitialize service - timeout or error");
        return false;
    }


    std::this_thread::sleep_for(std::chrono::seconds(3));
    // 检查服务是否可用
    if (!reload_map_service_->wait_for_service(std::chrono::seconds(5))) {
        RCLCPP_ERROR(this->get_logger(), "Service /localizer/relocalize not available after waiting");
        return false;
    }

    // 创建请求
    auto request = std::make_shared<interface::srv::Relocalize::Request>();
    request->pcd_path = pcd_name;
    request->x = initial_pose[0];
    request->y = initial_pose[1];
    request->z = initial_pose[2];
    request->yaw = initial_pose[3];
    RCLCPP_INFO(this->get_logger(),"initial pose: x=%.2f, y=%.2f, z=%.2f, yaw=%.2f",initial_pose[0],initial_pose[1],initial_pose[2],initial_pose[3]);
    RCLCPP_INFO(this->get_logger(), "Calling /localizer/relocalize service with pcd: %s", pcd_name.c_str());

    // 发送异步请求
    auto result_future = reload_map_service_->async_send_request(request);
    
    // return true;
    
    // 使用 wait_for 等待响应（避免使用 spin_until_future_complete）
    auto status = result_future.wait_for(std::chrono::seconds(10));
    if (status == std::future_status::ready) {
        auto response = result_future.get();

        if (response->success) {
            RCLCPP_INFO(this->get_logger(), "Relocalize service call successful: %s", response->message.c_str());
            return true;
        } else {
            RCLCPP_WARN(this->get_logger(), "Relocalize service call failed: %s", response->message.c_str());
            return false;
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to call /localizer/relocalize service - timeout or error");
        return false;
    }
}

// 计算欧氏距离
double TopoNavNode::calculate_euclidean_distance(float x1, float y1, float z1, float x2, float y2, float z2) {
    float dx = x1 - x2;
    float dy = y1 - y2;
    float dz = z1 - z2;
    return std::sqrt(dx * dx + dy * dy);
}

// 查找最近的拓扑节点
std::string TopoNavNode::find_nearest_node(float x, float y, float z) {
    std::string nearest_node = "";
    double min_distance = std::numeric_limits<double>::max();

    for (const auto& [node_name, node_data] : road_network) {
        double distance = calculate_euclidean_distance(x, y, z, node_data.param[0], node_data.param[1], node_data.param[2]);
        if (distance < min_distance) {
            min_distance = distance;
            nearest_node = node_name;
        }
    }

    RCLCPP_INFO(this->get_logger(), "Nearest node to (%.2f, %.2f, %.2f) is '%s' at distance %.3f m", x, y, z, nearest_node.c_str(), min_distance);

    return nearest_node;
}

// A*搜索算法
std::vector<std::string> TopoNavNode::astar_search(const std::string& start, const std::string& goal) {
    std::vector<std::string> path;

    // 检查起点和终点是否存在
    if (road_network.find(start) == road_network.end() || road_network.find(goal) == road_network.end()) {
        RCLCPP_ERROR(this->get_logger(), "Start or goal node not found in road network");
        return path;
    }

    // 优先队列(最小堆)
    std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode>> open_set;

    // 记录已访问的节点
    std::unordered_map<std::string, bool> closed_set;

    // 记录每个节点的最佳g_cost
    std::unordered_map<std::string, double> best_g_cost;

    // 记录父节点
    std::unordered_map<std::string, std::string> parent_map;

    // 计算启发式代价(起点到终点的欧氏距离)
    const auto& start_node = road_network[start];
    const auto& goal_node = road_network[goal];
    double h_start = calculate_euclidean_distance(start_node.param[0], start_node.param[1], start_node.param[2], goal_node.param[0],
                                                  goal_node.param[1], goal_node.param[2]);

    // 将起点加入开放列表
    open_set.push(AStarNode(start, 0.0, h_start, ""));
    best_g_cost[start] = 0.0;

    RCLCPP_INFO(this->get_logger(), "Starting A* search from '%s' to '%s'", start.c_str(), goal.c_str());

    while (!open_set.empty()) {
        // 取出f_cost最小的节点
        AStarNode current = open_set.top();
        open_set.pop();

        // 如果已经访问过,跳过
        if (closed_set[current.node_name]) {
            continue;
        }

        // 标记为已访问
        closed_set[current.node_name] = true;
        parent_map[current.node_name] = current.parent;

        RCLCPP_DEBUG(this->get_logger(), "Exploring node '%s' with f_cost=%.3f (g=%.3f, h=%.3f)", current.node_name.c_str(), current.f_cost,
                     current.g_cost, current.h_cost);

        // 找到目标节点
        if (current.node_name == goal) {
            RCLCPP_INFO(this->get_logger(), "Path found! Total cost: %.3f", current.g_cost);

            // 回溯路径
            std::string node = goal;
            while (!node.empty()) {
                path.push_back(node);
                node = parent_map[node];
            }
            std::reverse(path.begin(), path.end());

            // 打印路径
            std::string path_str = "";
            for (size_t i = 0; i < path.size(); ++i) {
                path_str += path[i];
                if (i < path.size() - 1)
                    path_str += " -> ";
            }
            RCLCPP_INFO(this->get_logger(), "Path: %s", path_str.c_str());

            return path;
        }

        // 扩展邻居节点
        const auto& current_node_data = road_network[current.node_name];
        for (size_t i = 0; i < current_node_data.connected_nodes.size(); ++i) {
            const std::string& neighbor = current_node_data.connected_nodes[i];

            // 如果邻居已经访问过,跳过
            if (closed_set[neighbor]) {
                continue;
            }

            // 计算到邻居的g_cost
            double edge_cost = current_node_data.length_list[i];
            double new_g_cost = current.g_cost + edge_cost;

            // 如果找到更好的路径,或者是第一次访问该节点
            if (best_g_cost.find(neighbor) == best_g_cost.end() || new_g_cost < best_g_cost[neighbor]) {
                best_g_cost[neighbor] = new_g_cost;

                // 计算启发式代价
                const auto& neighbor_node = road_network[neighbor];
                double h_cost = calculate_euclidean_distance(neighbor_node.param[0], neighbor_node.param[1], neighbor_node.param[2],
                                                             goal_node.param[0], goal_node.param[1], goal_node.param[2]);

                open_set.push(AStarNode(neighbor, new_g_cost, h_cost, current.node_name));
            }
        }
    }

    RCLCPP_ERROR(this->get_logger(), "No path found from '%s' to '%s'", start.c_str(), goal.c_str());
    return path;
}

// 四元数转yaw角
double TopoNavNode::quaternion_to_yaw(double qx, double qy, double qz, double qw) {
    return std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
}

// 归一化角度差 (处理角度环绕)
double TopoNavNode::normalize_angle_difference(double angle1, double angle2) {
    double diff = std::abs(angle1 - angle2);
    if (diff > M_PI) {
        diff = 2.0 * M_PI - diff;
    }
    return diff;
}

// 计算机器人到目标位姿的距离
double TopoNavNode::calculate_distance_to_pose(const geometry_msgs::msg::PoseStamped& pose) {
    // std::cout << "Robot Pose: (" << robot_pose_.pose.position.x << ", " << robot_pose_.pose.position.y << ", " << robot_pose_.pose.position.z << ")"
    //           << std::endl;

    return calculate_euclidean_distance(robot_pose_.pose.position.x, robot_pose_.pose.position.y, robot_pose_.pose.position.z, pose.pose.position.x,
                                        pose.pose.position.y, pose.pose.position.z);
}

// 检查是否到达航点
bool TopoNavNode::check_waypoint_reached(const geometry_msgs::msg::PoseStamped& goal_pose, double distance_threshold, double angle_threshold) {

    
    // 计算距离
    double distance = calculate_distance_to_pose(goal_pose);
    back_heart_msg.nav_dis = distance;
    // 计算角度差
    double robot_yaw = quaternion_to_yaw(robot_pose_.pose.orientation.x, robot_pose_.pose.orientation.y, robot_pose_.pose.orientation.z,
                                         robot_pose_.pose.orientation.w);

    double goal_yaw =
        quaternion_to_yaw(goal_pose.pose.orientation.x, goal_pose.pose.orientation.y, goal_pose.pose.orientation.z, goal_pose.pose.orientation.w);

    double angle_diff = normalize_angle_difference(robot_yaw, goal_yaw);

    // 检查是否满足阈值条件
    if (distance < distance_threshold && angle_diff < angle_threshold) {
        RCLCPP_INFO(this->get_logger(), "✓ Reached waypoint (dist: %.3f m, angle: %.1f deg)", distance, angle_diff * 180.0 / M_PI);
        return true;
    }

    // 定期打印状态
    static auto last_print = this->now();
    if ((this->now() - last_print).seconds() > 3.0) {
        RCLCPP_INFO(this->get_logger(), "Moving to waypoint: dist=%.2f m, angle=%.1f deg", distance, angle_diff * 180.0 / M_PI);
        last_print = this->now();
    }

    return false;
}

// 创建目标位姿消息
geometry_msgs::msg::PoseStamped TopoNavNode::create_goal_pose(float x, float y, float z, float yaw) {
    geometry_msgs::msg::PoseStamped goal_pose;
    goal_pose.header.stamp = this->now();
    goal_pose.header.frame_id = map_frame_;

    goal_pose.pose.position.x = x;
    goal_pose.pose.position.y = y;
    goal_pose.pose.position.z = z;

    // 将yaw转换为四元数
    goal_pose.pose.orientation.x = 0.0;
    goal_pose.pose.orientation.y = 0.0;
    goal_pose.pose.orientation.z = std::sin(yaw / 2.0);
    goal_pose.pose.orientation.w = std::cos(yaw / 2.0);

    return goal_pose;
}

// 修改：使用发布空目标的方式停止导航
void TopoNavNode::stop_current_navigation() {
    RCLCPP_INFO(this->get_logger(), "Stopping current navigation by publishing empty goal...");
    
    // 发布一个无效的目标位姿来停止导航
    // 大多数导航系统会因为接收到新目标而中断当前导航
    geometry_msgs::msg::PoseStamped empty_goal;
    empty_goal.header.stamp = this->now();
    empty_goal.header.frame_id = map_frame_;
    empty_goal.pose.position.x = 9999;
    empty_goal.pose.position.y = 9999;
    empty_goal.pose.position.z = 9999;
    empty_goal.pose.orientation = robot_pose_.pose.orientation;
    
    // 发布当前位置作为目标，实际上是停止移动
    goal_publisher_->publish(empty_goal);
    
    RCLCPP_INFO(this->get_logger(), "✓ Navigation stop command sent");
}

//TODO 发布导航路径
void TopoNavNode::publish_navigation_path(const std::vector<std::string>& path, const std::vector<float>& final_pose) {
    if (path.empty()) {
        RCLCPP_ERROR(this->get_logger(), "Cannot publish empty path");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "Publishing navigation path with %zu waypoints", path.size() + 1);

    // ========== 构建完整导航路径 ==========
    nav_msgs::msg::Path nav_path;
    nav_path.header.stamp = this->now();
    nav_path.header.frame_id = map_frame_;

    // 存储所有航点用于导航
    std::vector<geometry_msgs::msg::PoseStamped> waypoints;

    // 添加所有拓扑点到路径
    for (size_t i = 0; i < path.size(); ++i) {
        const auto& node_name = path[i];
        const auto& node_data = road_network[node_name];

        geometry_msgs::msg::PoseStamped pose;
        pose.header.stamp = this->now();
        pose.header.frame_id = map_frame_;
        pose.pose.position.x = node_data.param[0];
        pose.pose.position.y = node_data.param[1];
        pose.pose.position.z = 0.0;

        // 计算朝向
        float yaw;
        if (i < path.size() - 1) {  
            const auto& next_node = road_network[path[i + 1]];
            float dx = next_node.param[0] - node_data.param[0];
            float dy = next_node.param[1] - node_data.param[1];
            yaw = std::atan2(dy, dx);
        } else {
            // 最后一个点，保持与前一个节点的方向一致
            const auto& last_node = road_network[path[i - 1]];
            float dx = node_data.param[0] - last_node.param[0];
            float dy = node_data.param[1] - last_node.param[1];
            yaw = std::atan2(dy, dx);
        }

        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = std::sin(yaw / 2.0);
        pose.pose.orientation.w = std::cos(yaw / 2.0);

        nav_path.poses.push_back(pose);
        waypoints.push_back(pose);
    }

    // 添加最终目标点
    geometry_msgs::msg::PoseStamped final_goal_pose;
    final_goal_pose.header.stamp = this->now();
    final_goal_pose.header.frame_id = map_frame_;
    final_goal_pose.pose.position.x = final_pose[0];
    final_goal_pose.pose.position.y = final_pose[1];
    final_goal_pose.pose.position.z = 0.0;
    final_goal_pose.pose.orientation.x = 0.0;
    final_goal_pose.pose.orientation.y = 0.0;
    final_goal_pose.pose.orientation.z = std::sin(final_pose[3] / 2.0);
    final_goal_pose.pose.orientation.w = std::cos(final_pose[3] / 2.0);
    
    nav_path.poses.push_back(final_goal_pose);
    waypoints.push_back(final_goal_pose);

    // 发布完整路径用于可视化
    path_publisher_->publish(nav_path);
    // RCLCPP_INFO(this->get_logger(), "✓ Published complete navigation path with %zu poses for visualization", nav_path.poses.size());

    // 打印路径摘要
    std::string path_summary = "Path: ";
    for (size_t i = 0; i < path.size(); ++i) {
        path_summary += path[i];
        if (i < path.size() - 1)
            path_summary += " -> ";
    }
    path_summary += " -> GOAL";
    RCLCPP_INFO(this->get_logger(), "%s", path_summary.c_str());

    // ========== 使用已计算的路径进行导航 ==========
    const double distance_threshold = 0.5;
    const double angle_threshold = 0.5;
    const double final_distance_threshold = 0.3;
    const double final_angle_threshold = 0.3;
    
    rclcpp::Rate rate(10);  // 10Hz检查频率

    // 遍历所有航点（包括中间点和最终目标）
    for (size_t i = 0; i < waypoints.size(); ++i) {
        // 检查是否需要停止
        if (should_stop_navigation()) {
            RCLCPP_WARN(this->get_logger(), "Navigation stopped by user request");
            return;
        }

        const auto& waypoint = waypoints[i];
        
        // 判断是否为最终目标点
        bool is_final_goal = (i == waypoints.size() - 1);
        double current_distance_threshold = is_final_goal ? final_distance_threshold : distance_threshold;
        double current_angle_threshold = is_final_goal ? final_angle_threshold : angle_threshold;

        // 发布导航点（带时间间隔检测）
        auto current_time = this->now();
        const double min_interval_sec = 0.5; // 500 ms
        if ((current_time - last_goal_publish_time_).seconds() >= min_interval_sec) {
            goal_publisher_->publish(waypoint);
            // std::this_thread::sleep_for(100ms);
            // goal_publisher_->publish(waypoint);
            last_goal_publish_time_ = this->now();
            
            // 更新位置检测基准
            update_position_baseline();
            RCLCPP_INFO(this->get_logger(), "Published navigation pose at %.2f seconds", (current_time).seconds());
        } else {
            RCLCPP_DEBUG(this->get_logger(), "Goal publish throttled (min interval: 200ms)");
            std::this_thread::sleep_for(200ms);
            goal_publisher_->publish(waypoint);
            
            // 更新位置检测基准
            update_position_baseline();
            
            RCLCPP_INFO(this->get_logger(), "Published navigation pose at %.2f seconds", (current_time).seconds());
        }
        
        if (is_final_goal) {
            RCLCPP_INFO(this->get_logger(), "Published final goal at (%.2f, %.2f, %.2f)", 
                        waypoint.pose.position.x, waypoint.pose.position.y, waypoint.pose.position.z);
        } else {
            RCLCPP_INFO(this->get_logger(), "Published waypoint %zu/%zu: '%s' at (%.2f, %.2f, %.2f)", 
                        i + 1, waypoints.size(), path[i].c_str(),
                        waypoint.pose.position.x, waypoint.pose.position.y, waypoint.pose.position.z);
        }

        // 等待机器人到达当前航点
        // RCLCPP_INFO(this->get_logger(), "Waiting for robot to reach %s...", 
        //             is_final_goal ? "final goal" : "waypoint");
        
        // 新增：用于跟踪是否已重发过导航点
        bool goal_republished = false;
        // auto wait_start_time = this->now();

        //TODO 检查是否到达目标
        while (rclcpp::ok() && !should_stop_navigation()) {
            if (should_stop_navigation()) {
                RCLCPP_WARN(this->get_logger(), "Navigation interrupted by stop request");
                return;
            }
            if (check_waypoint_reached(waypoint, current_distance_threshold, current_angle_threshold)) {
                // 到达目标点后停止导航
                if (is_final_goal) {
                    RCLCPP_INFO(this->get_logger(), "Reached final goal, stopping navigation...");
                    stop_current_navigation();
                }
                break;
            }
            // 新增：检测机器人是否长时间未移动
            auto current_wait_time = this->now();
            if (!goal_republished && (current_wait_time - last_goal_publish_time_).seconds() > 1.0) {
                // 检查机器人位置是否变化
                if (!has_robot_moved(0.05)) {  // 5cm阈值
                    RCLCPP_WARN(this->get_logger(), 
                               "Robot hasn't moved for 0.5 second, republishing goal...");

                    // 重新发布导航点
                    goal_publisher_->publish(waypoint);
                    last_goal_publish_time_ = this->now();
                    
                    // 更新位置基准
                    update_position_baseline();
                    
                    // 标记已重发
                    // goal_republished = true;
                    
                    // 重置等待开始时间
                    // wait_start_time = this->now();
                    
                    RCLCPP_INFO(this->get_logger(), "Goal republished at %.2f seconds", 
                               this->now().seconds());
                } else {
                    // 机器人有移动，更新位置基准和等待开始时间
                    update_position_baseline();
                    // wait_start_time = this->now();
                    goal_republished = false;  // 重置重发标志
                }
            }
            rate.sleep();
        }

        if (should_stop_navigation()) {
            RCLCPP_WARN(this->get_logger(), "Navigation interrupted by stop request");
            return;
        }

        if (!rclcpp::ok()) {
            RCLCPP_WARN(this->get_logger(), "Navigation interrupted - ROS shutdown");
            return;
        }

        if (is_final_goal) {
            RCLCPP_INFO(this->get_logger(), "ALL Reached final goal################################!");
        } else {
            RCLCPP_INFO(this->get_logger(), "✓ Reached waypoint %zu/%zu", i + 1, waypoints.size() - 1);
        }
    }
    // back_heart_msg.nav_finish = true;
    RCLCPP_INFO(this->get_logger(), "========== Navigation Completed ==========");
}

// 根据ID查找节点名称
std::string TopoNavNode::find_node_by_id(const std::string& id) {
    RCLCPP_INFO(this->get_logger(), "Searching for node with ID '%s'", id.c_str());
    for (const auto& [node_name, node_data] : road_network) {
        if (node_data.id == id) {
            RCLCPP_INFO(this->get_logger(), "Found node '%s' with ID '%s'", node_name.c_str(), id.c_str());
            return node_name;
        }
    }
    RCLCPP_WARN(this->get_logger(), "No node found with ID '%s'", id.c_str());
    return "";
}

// 导航到指定节点
bool TopoNavNode::navigate_to_node(const std::string& target_node_name, const std::string& direction,const bool is_stairs) {
    if (road_network.find(target_node_name) == road_network.end()) {
        RCLCPP_ERROR(this->get_logger(), "Target node '%s' not found in road network", target_node_name.c_str());
        return false;
    }

    // 查找机器人当前位置最近的拓扑点
    std::string start_node;
    if(is_stairs){
        start_node = find_node_by_id(direction == "UP" ? "1" : "2");
        std::cout<<"楼梯导航，查找指定点"<<std::endl;
    }else{
        std::cout<<"楼层导航，查找最近点"<<std::endl;
        start_node = find_nearest_node(robot_pose_.pose.position.x, robot_pose_.pose.position.y, robot_pose_.pose.position.z);

    }

    if (start_node.empty()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to find nearest node to robot position");
        return false;
    }

    // 使用A*算法搜索路径
    if(start_node == target_node_name){
        RCLCPP_WARN(this->get_logger(), "Robot is already at the target node '%s'", target_node_name.c_str());
        return false;
    }
    std::vector<std::string> path = astar_search(start_node, target_node_name);

    if (path.empty()) {
        RCLCPP_ERROR(this->get_logger(), "A* search failed to find a path to '%s'", target_node_name.c_str());
        return false;
    }

    // 获取目标节点的位姿作为最终目标
    const auto& target_node = road_network[target_node_name];
    std::vector<float> final_pose = {target_node.param[0], target_node.param[1], target_node.param[2], target_node.param[3]};

    pLine();
    std::cout << "调整前yaw " << final_pose[3] << std::endl;
    if (direction == "DOWN") {
        std::cout << "下楼，调整方向" << std::endl;
        double new_yaw_down = final_pose[3] + 3.14159;
        new_yaw_down = fmod(new_yaw_down + 3.14159, 2 * 3.14159) - 3.14159;  // 归一化到[-pi,pi]
        final_pose[3] = new_yaw_down;
        std::cout << "调整后yaw: " << final_pose[3] << std::endl;
    }
    pLine();

    // 发布导航路径
    publish_navigation_path(path, final_pose);

    return true;
}

// 搜索楼层间路径 (使用BFS或Dijkstra算法)
std::vector<std::string> TopoNavNode::search_floor_path(const std::string& start_map, const std::string& goal_map) {
    std::vector<std::string> path;

    // 检查起点和终点是否存在
    if (map_topology.find(start_map) == map_topology.end() || map_topology.find(goal_map) == map_topology.end()) {
        RCLCPP_ERROR(this->get_logger(), "Start map '%s' or goal map '%s' not found in topology", start_map.c_str(), goal_map.c_str());
        return path;
    }

    // 使用Dijkstra算法搜索最短路径
    std::map<std::string, double> distances;
    std::map<std::string, std::string> parents;
    std::map<std::string, bool> visited;

    // 初始化距离
    for (const auto& [map_name, _] : map_topology) {
        distances[map_name] = std::numeric_limits<double>::max();
        visited[map_name] = false;
    }
    distances[start_map] = 0.0;

    RCLCPP_INFO(this->get_logger(), "Searching floor path from '%s' to '%s'", start_map.c_str(), goal_map.c_str());

    // Dijkstra算法主循环
    for (size_t i = 0; i < map_topology.size(); ++i) {
        // 找到未访问节点中距离最小的
        std::string current = "";
        double min_distance = std::numeric_limits<double>::max();

        for (const auto& [map_name, dist] : distances) {
            if (!visited[map_name] && dist < min_distance) {
                min_distance = dist;
                current = map_name;
            }
        }

        if (current.empty() || min_distance == std::numeric_limits<double>::max()) {
            break;
        }

        visited[current] = true;

        // 找到目标
        if (current == goal_map) {
            break;
        }

        // 更新邻居节点的距离
        const auto& current_edge = map_topology[current];
        for (size_t j = 0; j < current_edge.connected_nodes.size(); ++j) {
            const std::string& neighbor = current_edge.connected_nodes[j];
            double edge_cost = current_edge.length_list[j];
            double new_distance = distances[current] + edge_cost;

            if (new_distance < distances[neighbor]) {
                distances[neighbor] = new_distance;
                parents[neighbor] = current;
            }
        }
    }

    // 检查是否找到路径
    if (distances[goal_map] == std::numeric_limits<double>::max()) {
        RCLCPP_ERROR(this->get_logger(), "No floor path found from '%s' to '%s'", start_map.c_str(), goal_map.c_str());
        return path;
    }

    // 回溯路径
    std::string current = goal_map;
    while (current != start_map) {
        path.push_back(current);
        current = parents[current];
    }
    path.push_back(start_map);
    std::reverse(path.begin(), path.end());

    // 打印路径
    std::string path_str = "";
    for (size_t i = 0; i < path.size(); ++i) {
        path_str += path[i];
        if (i < path.size() - 1)
            path_str += " -> ";
    }
    RCLCPP_INFO(this->get_logger(), "Floor path: %s (total cost: %.3f)", path_str.c_str(), distances[goal_map]);

    return path;
}


//TODO 执行跨楼层导航
bool TopoNavNode::execute_cross_floor_navigation(const std::vector<std::string>& floor_path,
                                                 const std::string& start_map,
                                                 const std::string& goal_map,
                                                 const std::vector<float>& final_pose) {
    if (floor_path.size() < 2) {
        RCLCPP_ERROR(this->get_logger(), "Invalid floor path with less than 2 floors");
        return false;
    }
    pLine();
    RCLCPP_INFO(this->get_logger(), "Starting cross-floor navigation with %zu floors", floor_path.size());

    // 判断是上楼还是下楼
    int start_floor_level = map_topology[start_map].floor_level;
    int goal_floor_level = map_topology[goal_map].floor_level;
    bool is_going_up = start_floor_level < goal_floor_level;  // true: 上楼, false: 下楼

    RCLCPP_INFO(this->get_logger(), "Direction: %s (start floor %d -> goal floor %d)", is_going_up ? "Going UP" : "Going DOWN", start_floor_level,
                goal_floor_level);

    // 根据上下楼方向确定ID
    // 上楼: 起始层→ID1, 途经层ID1→ID2, 最后层ID1→目标
    // 下楼: 起始层→ID2, 途经层ID2→ID1, 最后层ID2→目标
    std::string floor_change = "2";  // 当前层楼梯点
    std::string transit_start_id = is_going_up ? "1" : "2";       // 途经层起点ID
    std::string transit_end_id = is_going_up ? "2" : "1";         // 途经层终点ID

    // 遍历楼层路径
    for (size_t i = 0; i < floor_path.size() - 1; ++i) {
        // 检查是否需要停止
        if (should_stop_navigation()) {
            RCLCPP_WARN(this->get_logger(), "Cross-floor navigation stopped by user request");
            return false;
        }

        const std::string& current_floor = floor_path[i];
        const std::string& next_floor = floor_path[i + 1];


        RCLCPP_INFO(this->get_logger(), "===== Floor %zu/%zu: Navigating on '%s' =====", i + 1, floor_path.size(), current_floor.c_str());

        // 确定目标节点
        std::string target_node;
        std::string Direction = is_going_up ? "UP" : "DOWN";

        ////////////////////////////////////第一层
        if (i == 0) {
            back_heart_msg.curr_floor = current_floor;
            // 加载当前楼层的拓扑地图配置文件
            std::string floor_config = config_dir_ + "/" + current_floor + "_topo.yaml";
            if (!load_floor_topology(floor_config)) {
                RCLCPP_ERROR(this->get_logger(), "Failed to load floor topology for '%s'", current_floor.c_str());
                return false;
            }

            /////////////////////////////////第一层导航到切换点
            target_node = find_node_by_id(floor_change);
            std::cout<<"第一层，导航到楼梯点 ID="<<floor_change<<std::endl;
            if (target_node.empty()) {
                RCLCPP_ERROR(this->get_logger(), "Failed to find target node on floor '%s'", current_floor.c_str());
                return false;
            }

            // 导航到目标节点
            if (!navigate_to_node(target_node, Direction,false)) {
                RCLCPP_ERROR(this->get_logger(), "Failed to navigate to node '%s' on floor '%s'", target_node.c_str(), current_floor.c_str());
                return false;
            }
            RCLCPP_INFO(this->get_logger(), "--------------------第一层导航结束---------------------");
        } else {
            ////////////////////////////////////中间层导航
            // 中间层(楼梯): 根据上下楼方向确定导航方向
            // 上楼: ID 1 (start) → ID 2 (end)
            // 下楼: ID 2 (end) → ID 1 (start)
            target_node = find_node_by_id(transit_end_id);
            RCLCPP_INFO(this->get_logger(), "中间层，导航到楼梯点 ID=%s", transit_end_id.c_str());
            if (target_node.empty()) {
                RCLCPP_ERROR(this->get_logger(), "Failed to find target node on floor '%s'", current_floor.c_str());
                return false;
            }
            
            RCLCPP_INFO(this->get_logger(), "当前%s楼层等级: %d",current_floor.c_str(), road_network[current_floor].floor_level);
            if (current_floor.find("stairs") != std::string::npos){//包含楼梯
            // if(road_network[current_floor].floor_level==99) {
                RCLCPP_INFO(this->get_logger(), "********************楼梯导航***********************");
                std_msgs::msg::Bool scan_filter_trig;
                scan_filter_trig.data = true;
                scan_filter_publisher_->publish(scan_filter_trig);
                // if (current_floor.find("stairs") != std::string::npos){//包含楼梯
                // 导航到目标节点
                if (!navigate_to_node(target_node, Direction,true)) {
                    RCLCPP_ERROR(this->get_logger(), "Failed to navigate to node '%s' on floor '%s'", target_node.c_str(), current_floor.c_str());
                    return false;
                }
            }else{
                // 导航到目标节点
                RCLCPP_INFO(this->get_logger(), "********************楼层导航***********************");
                target_node = find_node_by_id(floor_change);
                if (!navigate_to_node(target_node, Direction,false)) {
                    RCLCPP_ERROR(this->get_logger(), "Failed to navigate to node '%s' on floor '%s'", target_node.c_str(), current_floor.c_str());
                    return false;
                }
            }
            // // 导航到目标节点
            // if (!navigate_to_node(target_node, Direction,true)) {
            //     RCLCPP_ERROR(this->get_logger(), "Failed to navigate to node '%s' on floor '%s'", target_node.c_str(), current_floor.c_str());
            //     return false;
            // }
        }
        // 检查是否需要停止
        if (should_stop_navigation()) {
            RCLCPP_WARN(this->get_logger(), "Cross-floor navigation stopped after reaching node");
            return false;
        }

        // RCLCPP_INFO(this->get_logger(), "✓ Reached target node '%s' on floor '%s'", target_node.c_str(), current_floor.c_str());

        ///////////////////////////// 到达楼层切换点，停止导航
        RCLCPP_INFO(this->get_logger(), "Stopping navigation before floor change...");
        stop_current_navigation();
        std::this_thread::sleep_for(300ms);  // 等待停止生效

        ///////////////////////////// 如果不是最后一层，需要切换地图
        if (i < floor_path.size() - 2) {
            RCLCPP_INFO(this->get_logger(), "--------------------中间层---------------------");
            // 记载拓扑路网
            back_heart_msg.curr_floor = next_floor;
            std::string floor_config = config_dir_ + "/" + next_floor + "_topo.yaml";
            if (!load_floor_topology(floor_config)) {
                RCLCPP_ERROR(this->get_logger(), "Failed to load floor topology for '%s'", next_floor.c_str());
                return false;
            }

            // TODO 切换到下一层的地图
            std::string next_pcd = pcd_dir_ + "/" + next_floor + ".pcd";

            RCLCPP_INFO(this->get_logger(), "Switching to next floor: '%s'", next_floor.c_str());

            // 初始化定位点
            std::vector<float> init_pose(4, 0.0f);                                 // x, y, z, yaw
            if (is_going_up) {                                                     // 上楼，则初始化点在start位置
                std::string target_node_name = find_node_by_id(transit_start_id);  // 起始点
                // 拿到位置信息进行位置初始化
                const auto& target_node = road_network[target_node_name];
                init_pose[0] = target_node.param[0];
                init_pose[1] = target_node.param[1];
                init_pose[2] = target_node.param[2];
                init_pose[3] = target_node.param[4];
            } else {                                                               // 下楼，则初始化点在end位置
                std::string target_node_name = find_node_by_id(transit_start_id);  // 结束点
                // 拿到位置信息进行位置初始化
                const auto& target_node = road_network[target_node_name];
                init_pose[0] = target_node.param[0];
                init_pose[1] = target_node.param[1];
                init_pose[2] = target_node.param[2];

                init_pose[3] = 3.14159 + target_node.param[3];                       // 朝向调整180度
                init_pose[3] = fmod(init_pose[3] + 3.14159, 2 * 3.14159) - 3.14159;  // 归一化到[-pi,pi]
            }
            // //同楼层地图切换
            // if(road_network[current_floor].floor_level== road_network[floor_path[i-1]].floor_level){
            //     RCLCPP_INFO(this->get_logger(), "********************同楼层地图切换***********************");
            //     std::string target_node_name = find_node_by_id(floor_change);  // 起始点
            //     // 拿到位置信息进行位置初始化
            //     const auto& target_node = road_network[target_node_name];
            //     init_pose[0] = target_node.param[0];
            //     init_pose[1] = target_node.param[1];
            //     init_pose[2] = target_node.param[2];
            //     init_pose[3] = target_node.param[4];
            // }

            //TODO 调用地图切换服务
            if (!call_change_pcd_service(next_pcd, init_pose)) {
                RCLCPP_ERROR(this->get_logger(), "Failed to switch to floor '%s'", next_floor.c_str());
                return false;
            }

            // 等待一段时间让系统稳定
            RCLCPP_INFO(this->get_logger(), "Waiting for localization to stabilize...");
            rclcpp::sleep_for(std::chrono::seconds(3));
        }
    }
    RCLCPP_INFO(this->get_logger(), "-----------------最后一层导航------------------------");
    //最后一层导航
    std_msgs::msg::Bool scan_filter_trig;
    scan_filter_trig.data = false;
    scan_filter_publisher_->publish(scan_filter_trig);

    // 最后一层: 导航到最终目标
    const std::string& final_floor = floor_path.back();
    RCLCPP_INFO(this->get_logger(), "===== Final Floor: Navigating to goal on '%s' =====", final_floor.c_str());

    // 加载最后一层的拓扑地图
    back_heart_msg.curr_floor = final_floor;
    std::string final_floor_config = config_dir_ + "/" + final_floor + "_topo.yaml";
    if (!load_floor_topology(final_floor_config)) {
        RCLCPP_ERROR(this->get_logger(), "Failed to load floor topology for '%s'", final_floor.c_str());
        return false;
    }

    // 切换到最后一层的地图
    std::string final_pcd = pcd_dir_ + "/" + final_floor + ".pcd";
    // 初始化定位点
    std::vector<float> init_pose(4, 0.0f);                                 // x, y, z, yaw
    if (is_going_up) {                                                     // 上楼，则初始化点在start位置
        std::string target_node_name = find_node_by_id(transit_start_id);  // 起始点
        // 拿到位置信息进行位置初始化
        const auto& target_node = road_network[target_node_name];
        init_pose[0] = target_node.param[0];
        init_pose[1] = target_node.param[1];
        init_pose[2] = target_node.param[2];
        init_pose[3] = target_node.param[4];
    } else {                                                               // 下楼，则初始化点在end位置
        std::string target_node_name = find_node_by_id(transit_start_id);  // 结束点
        // 拿到位置信息进行位置初始化
        const auto& target_node = road_network[target_node_name];
        init_pose[0] = target_node.param[0];
        init_pose[1] = target_node.param[1];
        init_pose[2] = target_node.param[2];

        init_pose[3] = 3.14159 + target_node.param[3];                       // 朝向调整180度
        init_pose[3] = fmod(init_pose[3] + 3.14159, 2 * 3.14159) - 3.14159;  // 归一化到[-pi,pi]
    }

    if (!call_change_pcd_service(final_pcd, init_pose)) {
        RCLCPP_ERROR(this->get_logger(), "Failed to switch to final floor '%s'", final_floor.c_str());
        return false;
    }

    RCLCPP_INFO(this->get_logger(), "Waiting for localization ############################");
    rclcpp::sleep_for(std::chrono::seconds(3));
    RCLCPP_INFO(this->get_logger(), "-----------------------------------------");
    // 查找从起始ID节点开始
    // 上楼: 从 ID=1 (start点) 开始
    // 下楼: 从 ID=2 (end点) 开始
    std::string start_node = find_node_by_id(floor_change);
    if (start_node.empty()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to find start node (ID=%s) on final floor", floor_change.c_str());
        return false;
    }

    // 查找距离最终目标最近的拓扑点
    std::string goal_node = find_nearest_node(final_pose[0], final_pose[1], final_pose[2]);
    if (goal_node.empty()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to find goal node on final floor");
        return false;
    }

    RCLCPP_INFO(this->get_logger(), "Final floor: navigating from ID=%s node '%s' (%s) to goal node '%s'", floor_change.c_str(),
                start_node.c_str(), is_going_up ? "start" : "end", goal_node.c_str());

    // 使用A*算法搜索路径
    std::vector<std::string> path = astar_search(start_node, goal_node);
    if (path.empty()) {
        RCLCPP_ERROR(this->get_logger(), "A* search failed on final floor");
        return false;
    }

    // 发布导航路径
    publish_navigation_path(path, final_pose);

    RCLCPP_INFO(this->get_logger(), "========== Cross-Floor Navigation Completed ==========");
    
    return true;
}

// 新增：检查机器人是否移动
bool TopoNavNode::has_robot_moved(double threshold) {
    std::lock_guard<std::mutex> lock(position_check_mutex_);
    
    double dx = robot_pose_.pose.position.x - last_robot_position_.x;
    double dy = robot_pose_.pose.position.y - last_robot_position_.y;
    double dz = robot_pose_.pose.position.z - last_robot_position_.z;
    
    double distance = std::sqrt(dx * dx + dy * dy + dz * dz);
    
    return distance > threshold;
}

// 新增：更新位置检测基准
void TopoNavNode::update_position_baseline() {
    std::lock_guard<std::mutex> lock(position_check_mutex_);
    
    last_robot_position_.x = robot_pose_.pose.position.x;
    last_robot_position_.y = robot_pose_.pose.position.y;
    last_robot_position_.z = robot_pose_.pose.position.z;
    last_position_check_time_ = this->now();
}

//TODO：后台导航线程函数
void TopoNavNode::navigation_thread_func(const std::shared_ptr<interface::srv::NavMsg::Request> request) {
    RCLCPP_INFO(this->get_logger(), "========== Navigation Thread Started ==========");
    RCLCPP_INFO(this->get_logger(), "  Start map: %s", request->start_map.c_str());
    RCLCPP_INFO(this->get_logger(), "  Stop map: %s", request->stop_map.c_str());
    RCLCPP_INFO(this->get_logger(), "  Nav pose: [%.2f, %.2f, %.2f, %.2f]", 
                request->nav_pose[0], request->nav_pose[1], request->nav_pose[2], request->nav_pose[3]);

    // 清空现有路网数据
    {
        std::lock_guard<std::mutex> lock(navigation_mutex_);
        road_network.clear();
        map_topology.clear();
    }
    init_heart_msg();

    back_heart_msg.call_start_floor=request->start_map;
    back_heart_msg.call_stop_floor=request->stop_map;
    back_heart_msg.curr_floor = request->start_map;

    std::cout << "路网数据已经清空" << std::endl;

    try {
        // TODO start == end - 同楼层导航
        if (request->start_map == request->stop_map) {
            RCLCPP_INFO(this->get_logger(), "===== Same Floor Navigation on %s =====", request->start_map.c_str());
            // 加载地图拓扑配置
            std::string node_config_file = config_dir_ + "/" + request->start_map + "_topo.yaml";
            std::cout << "加载节点配置文件: " << node_config_file << std::endl;
            
            if (!load_floor_topology(node_config_file)) {
                RCLCPP_ERROR(this->get_logger(), "Failed to load node topology from: %s", node_config_file.c_str());
                navigation_active_ = false;
                return;
            }

            RCLCPP_INFO(this->get_logger(), "Successfully loaded node topology from: %s", node_config_file.c_str());

            // 步骤1: 查找机器人当前位置最近的拓扑点
            std::string start_node = find_nearest_node(robot_pose_.pose.position.x, robot_pose_.pose.position.y, robot_pose_.pose.position.z);
            if (start_node.empty()) {
                RCLCPP_ERROR(this->get_logger(), "Failed to find nearest node to robot position");
                navigation_active_ = false;
                return;
            }

            // 步骤2: 查找 目标位置最近的拓扑点
            std::string goal_node = find_nearest_node(request->nav_pose[0], request->nav_pose[1], request->nav_pose[2]);
            if (goal_node.empty()) {
                RCLCPP_ERROR(this->get_logger(), "Failed to find nearest node to goal position");
                navigation_active_ = false;
                return;
            }

            RCLCPP_INFO(this->get_logger(), "Navigation: %s -> %s", start_node.c_str(), goal_node.c_str());

            // 步骤3: 使用A*算法搜索路径

            if(start_node == goal_node){
                RCLCPP_WARN(this->get_logger(), "Start node and goal node are the same: %s", start_node.c_str());
                return;
            }
            std::vector<std::string> path = astar_search(start_node, goal_node);

            if (path.size() < 1) {
                RCLCPP_ERROR(this->get_logger(), "A* search failed to find a path");
                navigation_active_ = false;
                return;
            }

            std::cout << "搜索到的路径节点如下: " << std::endl;
            for (const auto& node : path) {
                std::cout << "路径节点: " << node << std::endl;
            }

            // 步骤4: 目标点位姿
            std::vector<float> final_pose = {
                static_cast<float>(request->nav_pose[0]), 
                static_cast<float>(request->nav_pose[1]),
                static_cast<float>(request->nav_pose[2]), 
                static_cast<float>(request->nav_pose[3])
            };

            publish_navigation_path(path, final_pose);

            if (!should_stop_navigation()) {
                RCLCPP_INFO(this->get_logger(), "===== Navigation Completed Successfully =====");
            }

            back_heart_msg.nav_finish = true;
        }
        // TODO start != end - 跨楼层导航
        else {
            RCLCPP_INFO(this->get_logger(), "===== Cross-Floor Navigation: %s -> %s =====", 
                        request->start_map.c_str(), request->stop_map.c_str());

            // 加载地图拓扑配置
            std::string map_config_file = config_dir_ + "/" + map_topo_name_ + ".yaml";
            if (!load_map_topology(map_config_file)) {
                RCLCPP_ERROR(this->get_logger(), "Failed to load map topology from: %s", map_config_file.c_str());
                navigation_active_ = false;
                return;
            }

            RCLCPP_INFO(this->get_logger(), "Successfully loaded map topology from: %s", map_config_file.c_str());

            // 步骤1: 使用map_topology搜索楼层间的连接路径
            std::vector<std::string> floor_path = search_floor_path(request->start_map, request->stop_map);
            
            for (const auto& floor : floor_path) {
                std::cout << "connect floor: " << floor << std::endl;
            }

            if (floor_path.empty()) {
                RCLCPP_ERROR(this->get_logger(), "Failed to find floor path from '%s' to '%s'", 
                            request->start_map.c_str(), request->stop_map.c_str());
                navigation_active_ = false;
                return;
            }

            // 步骤2-5: 执行跨楼层导航
            std::vector<float> final_pose = {
                static_cast<float>(request->nav_pose[0]), 
                static_cast<float>(request->nav_pose[1]),
                static_cast<float>(request->nav_pose[2]), 
                static_cast<float>(request->nav_pose[3])
            };

            bool success = execute_cross_floor_navigation(floor_path, request->start_map, request->stop_map, final_pose);
            back_heart_msg.nav_finish = true;
            if (success && !should_stop_navigation()) {
                RCLCPP_INFO(this->get_logger(), "===== Cross-Floor Navigation Completed Successfully =====");
            } else if (!success) {
                RCLCPP_ERROR(this->get_logger(), "===== Cross-Floor Navigation Failed =====");
            }
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception in navigation thread: %s", e.what());
    }

    // 导航结束，重置标志
    navigation_active_ = false;
    stop_navigation_ = false;
    RCLCPP_INFO(this->get_logger(), "========== Navigation Thread Finished ==========");
}


// 导航服务多线程函数
void TopoNavNode::nav_service_callback(const std::shared_ptr<interface::srv::NavMsg::Request> request,
                                       std::shared_ptr<interface::srv::NavMsg::Response> response) {
    RCLCPP_INFO(this->get_logger(), "========== Received Navigation Request ==========");
    // 检查是否已有导航在进行
    if (navigation_active_.load()) {
        response->success = false;
        response->message = "Navigation already in progress. Please stop current navigation first.";
        RCLCPP_WARN(this->get_logger(), "Navigation request rejected: already in progress");
        return;
    }

    // 等待之前的导航线程结束
    if (navigation_thread_.joinable()) {
        navigation_thread_.join();
    }

    // 重置停止标志
    stop_navigation_ = false;
    
    // 设置导航活动标志
    navigation_active_ = true;

    // 启动后台导航线程
    navigation_thread_ = std::thread(&TopoNavNode::navigation_thread_func, this, request);

    // 快速返回响应
    response->success = true;
    response->message = "Navigation started successfully in background thread";
    RCLCPP_INFO(this->get_logger(), "Navigation thread started, service callback returning");
}

/****************************************************************************************************** */
int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<TopoNavNode>();

    RCLCPP_INFO(node->get_logger(), "Starting Topo Navigation Node...");
    
    // 使用多线程executor以支持并发
    // rclcpp::executors::MultiThreadedExecutor executor;
    // executor.add_node(node);
    // executor.spin();

    rclcpp::spin(node);
    rclcpp::shutdown();


    rclcpp::shutdown();
    return 0;
}
